% 验证卡尔曼滤波器基本估计能力 %TODO: 加速度计二次项系数有跳变

clc;
clear;
load('mcsim', 'in');
par.g = 0;
cst = load('earthconstants_virtual.mat', 'RE', 'e');
cst.deg2Rad = pi / 180;
cst.sec2Rad = pi / 3600 / 180;
in.curSimParVal(1:36, 1) = in.parRefVal(1:36, 2);

%% 生成轨迹
% 常量
cst_trajGen = load('earthconstants_virtual.mat', 'omegaIE', 'RE', 'epsilonSq');
% 配置
cfg_trajGen.enableTrajectoryGen = true;
cfg_trajGen.enableSensErr = true; % 在轨迹中引入器件误差
cfg_trajGen.enableSensRandErr = false;
cfg_trajGen.enableNavigation = false;
cfg_trajGen.ignore2OdSensErr = true;
cfg_trajGen.IMUOutFilePath = [];
cfg_trajGen.quantizeIMUOut = false;
cfg_trajGen.IMUOutPrecision = '%.16e';
% 参量
par_trajGen.samplePeriod = 0.01;
par_trajGen.g = par.g; % 重力加速度，m/s^2
par_trajGen.IMU.acc.B = in.curSimParVal(10:12) * 9.8 * 1e-6; % 转换前单位μg，转换后单位m/s^2
par_trajGen.IMU.acc.SF0 = ones(3, 1);
par_trajGen.IMU.acc.dSF = in.curSimParVal(13:15) * 1e-6;
par_trajGen.IMU.acc.MA = vec2offdiag(in.curSimParVal(16:21)) * 1e-6; % 转换前单位μrad，转换后单位rad
par_trajGen.IMU.acc.SO = in.curSimParVal(22:24, 1) * 1e-6 / 9.8; % 转换前单位μg/g^2，转换后单位1/(m/s^2)
par_trajGen.IMU.gyro.B = in.curSimParVal(25:27) * cst.sec2Rad; % 转换前单位°/h，转换后单位rad/s
par_trajGen.IMU.gyro.SF0 = ones(3, 1);
par_trajGen.IMU.gyro.dSFp = in.curSimParVal(28:30) * 1e-6;
par_trajGen.IMU.gyro.dSFn = par_trajGen.IMU.gyro.dSFp;
par_trajGen.IMU.gyro.MA = vec2offdiag(in.curSimParVal(31:36)) * 1e-6; % 转换前单位μrad，转换后单位rad
par_trajGen.IMU.gyro.GS = zeros(3);
% 输入量
in_trajGen.genFuncName = 'gentgi_filtervld';
in_trajGen.genFuncInArg = {};
% 初始条件
iniCon_trajGen.CBG = eye(3); % 初始姿态矩阵
iniCon_trajGen.vG = zeros(3, 1); % 初始速度
iniCon_trajGen.pos = [0 * cst.deg2Rad, 0 * cst.deg2Rad, 0]'; % 纬度、经度、高度
% 生成轨迹
[out_trajGen, ~] = gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen);

%% 运行组合导航
for i=1:5
    % 常量
    cst_intNav = load('earthconstants_virtual.mat', 'RE', 'e', 'mu', 'J2', 'J3', 'omegaIE', 'epsilonSq');
    % 配置
    cfg_intNav.enableKalmFilt = false;
    cfg_intNav.enableKFUpdate = false;
    cfg_intNav.extrapAlgo = KalmFiltExtrapAlgo.ITER;
    cfg_intNav.updAlgo = KalmFiltUpdAlgo.JOSEPH;
    switch i
        case 1
            cfg_intNav.useDCMInAttCalc = false;
            cfg_intNav.dynaMatINSSysBlkForm = 1;
            cfg_intNav.navCoordType = NavCoordType.WAN_AZM;
            navParErrFigName = '导航参数误差(四元数+φ+游动方位)'; % 四元数姿态解算 + φ形式误差方程 + 游动方位坐标系
            accParErrFigName = '加速度计参数(四元数+φ+游动方位)';
            gyroParErrFigName = '陀螺参数(四元数+φ+游动方位)';
        case 2
            cfg_intNav.useDCMInAttCalc = true;
            cfg_intNav.dynaMatINSSysBlkForm = 1;
            cfg_intNav.navCoordType = NavCoordType.WAN_AZM;
            navParErrFigName = '导航参数误差(方向余弦+φ+游动方位)'; % 方向余弦矩阵姿态解算 + φ形式误差方程 + 游动方位坐标系
            accParErrFigName = '加速度计参数(方向余弦+φ+游动方位)';
            gyroParErrFigName = '陀螺参数(方向余弦+φ+游动方位)';
        case 3
            cfg_intNav.useDCMInAttCalc = false;
            cfg_intNav.dynaMatINSSysBlkForm = 2;
            cfg_intNav.navCoordType = NavCoordType.WAN_AZM;
            navParErrFigName = '导航参数误差(四元数+ψ+游动方位)'; % 四元数姿态解算 + ψ形式误差方程 + 游动方位坐标系
            accParErrFigName = '加速度计参数(四元数+ψ+游动方位)';
            gyroParErrFigName = '陀螺参数(四元数+ψ+游动方位)';
        case 4
            cfg_intNav.useDCMInAttCalc = false;
            cfg_intNav.dynaMatINSSysBlkForm = 1;
            cfg_intNav.navCoordType = NavCoordType.FREE_AZM;
            navParErrFigName = '导航参数误差(四元数+φ+自由方位)'; % 四元数姿态解算 + φ形式误差方程 + 自由方位坐标系
            accParErrFigName = '加速度计参数(四元数+φ+自由方位)';
            gyroParErrFigName = '陀螺参数(四元数+φ+自由方位)';
        otherwise
            cfg_intNav.useDCMInAttCalc = false;
            cfg_intNav.dynaMatINSSysBlkForm = 1;
            cfg_intNav.navCoordType = NavCoordType.GEO;
            navParErrFigName = '导航参数误差(四元数+φ+ENU地理)'; % 四元数姿态解算 + φ形式误差方程 + ENU地理坐标系
            accParErrFigName = '加速度计参数(四元数+φ+ENU地理)';
            gyroParErrFigName = '陀螺参数(四元数+φ+ENU地理)';
    end
    cfg_intNav.sysStateMask = [true(1, 10) false(1, 1)];
    cfg_intNav.sensStateMask = [true(1, 27) false(1, 9)];
    cfg_intNav.enableNumCondCtrl = false;
    % 初始条件
    iniCon_intNav.pos = out_trajGen.pos(1, :)'; % 无误差
    iniCon_intNav.vN = out_trajGen.vG(1, :)'; % 无误差
    iniCon_intNav.CBN = out_trajGen.CBG(:, :, 1); % 无误差
    iniCon_intNav.kalmFilt.x = [-in.curSimParVal(1)/cst.RE; in.curSimParVal(2)/cst.RE; in.curSimParVal(3); in.curSimParVal(6:-1:4); in.curSimParVal(9:-1:7)*cst.sec2Rad; in.curSimParVal(2)/cst.RE*tan(out_trajGen.pos(1, 1));...
        in.curSimParVal(10:12)*9.8*1e-6; in.curSimParVal(13:21)*1e-6; in.curSimParVal(22:24)/9.8; in.curSimParVal(25:27)*cst.sec2Rad; in.curSimParVal(28:36)*1e-6];
    iniCon_intNav.kalmFilt.P = diag(iniCon_intNav.kalmFilt.x.^2);
    % 参量
    par_intNav.tBgn = out_trajGen.t(1);
    par_intNav.tEnd = out_trajGen.t(end);
    par_intNav.dT = par_trajGen.samplePeriod;
    par_intNav.tUpd = 1; % Aid组合导航的量测更新周期
    par_intNav.g = par_trajGen.g;
    par_intNav.INSCycPerKalFiltExtrapCyc = int32(10);
    par_intNav.GammaMRGammaMT = diag([0.001/cst.RE, 0.001/cst.RE, 0.001, 0.0001, 0.0001, 0.0001, 1e-7, 1e-7, 1e-7, 0.001/cst.RE].^2);
    par_intNav.INSSensRWC = eps(iniCon_intNav.kalmFilt.x(4:9, :));
    par_intNav.INSSensStateProcNoisePSDFull = [eps(iniCon_intNav.kalmFilt.x(11:end, :)); zeros(9, 1)]; % par_intNav.INSSensRWC及par_intNav.INSSensStateProcNoisePSDFull取0会导致in_kalmFilt.GPQGPT矩阵所有元素为0，进而导致out_kalmFilt.P对角线元素出现负数，故取eps而不是0
    par_intNav.PDiagMin = NaN(nnz([cfg_intNav.sysStateMask cfg_intNav.sensStateMask]), 1);
    par_intNav.PDiagMax = NaN(nnz([cfg_intNav.sysStateMask cfg_intNav.sensStateMask]), 1);
    % 输入量
    m = size(out_trajGen.accOutErrFree, 1);
    in_intNav.INS.accOut = out_trajGen.accOutErrFree;
    in_intNav.INS.gyroOut = out_trajGen.gyroOutErrFree;
    in_intNav.aid.CNE = zeros(3, 3, m);
    in_intNav.aid.vN = zeros(m, 3);
    in_intNav.aid.vG = out_trajGen.vG;
    in_intNav.aid.CBN = zeros(3, 3, m);
    in_intNav.aid.h = out_trajGen.pos(:, 3);
    in_intNav.aid.alpha = zeros(m, 1);
    in_intNav.traj.CNE = zeros(3, 3, m);
    in_intNav.traj.pos = out_trajGen.pos;
    in_intNav.traj.vN = out_trajGen.vG;
    in_intNav.traj.CBN = zeros(3, 3, m);
    in_intNav.traj.fB = out_trajGen.fBTrue;
    in_intNav.traj.omegaIBB = out_trajGen.omegaIBBTrue;
    in_intNav.traj.FCN = zeros(3, 3, m);
    in_intNav.traj.gN = zeros(m, 3);
    in_intNav.u = NaN(0, 1);
    % 运行导航
    clear('intnav_kfvld');
    % 纯惯导解算(TODO: 因为轨迹发生器无vN, CNE, CBN, FCN, gN, alpha参数输出，故利用纯惯导解算获取这些参数，后续待轨迹发生器完善之后可删除此段纯惯导解算代码)
    [out_inertNavIdeal, ~] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav);
    % 组合导航
    cfg_intNav.enableKalmFilt = true;
    cfg_intNav.enableKFUpdate = true;
    iniCon_intNav.kalmFilt.x = zeros(37, 1); % 初始状态为0
    [dL, dlambda, ~] = dposvec2dllwa(in.curSimParVal(2, 1), in.curSimParVal(1, 1), NaN, out_trajGen.pos(1, 1), out_inertNavIdeal.alpha(1, 1), out_inertNavIdeal.CNE(:, :, 1), out_trajGen.pos(1, 3), cst_intNav.RE, cst_intNav.e);
    iniCon_intNav.pos = out_trajGen.pos(1, :)' + [dL; dlambda; in.curSimParVal(3, 1)];
    iniCon_intNav.vN = out_trajGen.vG(1, :)' + in.curSimParVal(6:-1:4, 1);
    iniCon_intNav.CBN = angle2dcm(in.curSimParVal(7)*cst.sec2Rad, in.curSimParVal(8)*cst.sec2Rad, in.curSimParVal(9)*cst.sec2Rad) * out_trajGen.CBG(:, :, 1);
    in_intNav.INS.accOut = out_trajGen.accOut;
    in_intNav.INS.gyroOut = out_trajGen.gyroOut;
    in_intNav.aid.CNE = out_inertNavIdeal.CNE;
    in_intNav.aid.vN = out_inertNavIdeal.vN;
    in_intNav.aid.CBN = out_inertNavIdeal.CBN;
    in_intNav.aid.alpha = out_inertNavIdeal.alpha;
    in_intNav.traj.CNE = out_inertNavIdeal.CNE;
    in_intNav.traj.CBN = out_inertNavIdeal.CBN;
    in_intNav.traj.fB = out_trajGen.fBTrue;
    in_intNav.traj.omegaIBB = out_trajGen.omegaIBBTrue;
    in_intNav.traj.FCN = out_inertNavIdeal.FCN;
    in_intNav.traj.gN = out_inertNavIdeal.gN;
    [out_intNavError, auxOut_intNavError] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav);
    % 系统误差
    m = size(out_intNavError.kalmFilt_tExtrap, 1);
    CHatEN = transpose_nd(out_intNavError.CNE);
    CEN = transpose_nd(out_inertNavIdeal.CNE);
    posErr = out_intNavError.pos - out_inertNavIdeal.pos;
    phi = dcmdiff(out_intNavError.CBN, out_inertNavIdeal.CBN);
    dtheta = dcmdiff(CHatEN, CEN);
    if cfg_intNav.dynaMatINSSysBlkForm == 1
        vNErr = out_intNavError.vN - out_inertNavIdeal.vN;
        structNavPar(1) = navparerrarr2struct([dtheta(1:10:end, 1:2) posErr(1:10:end, 3) vNErr(1:10:end, :) phi(1:10:end, :) dtheta(1:10:end, 3)], [1:10 NaN(1, 5)], false);
    else
        psi = phi - dtheta;
        vNErr = (out_intNavError.vN - out_inertNavIdeal.vN) + cross(dtheta, out_inertNavIdeal.vN); % ψ形式误差方程与φ形式误差方程中速度误差参数的观测坐标系不同，需要转换
        structNavPar(1) = navparerrarr2struct([dtheta(1:10:end, 1:2) posErr(1:10:end, 3) vNErr(1:10:end, :) psi(1:10:end, :) dtheta(1:10:end, 3)], [1:10 NaN(1, 5)], false);
    end
    structNavPar(2) = navparerrarr2struct(out_intNavError.kalmFilt_x(:, 1:10), [1:10 NaN(1, 5)], false);
    plotnavparerr(out_intNavError.kalmFilt_tExtrap, structNavPar, {'误差真值', '误差估计值'}, [], [], [], navParErrFigName);
    disp(navParErrFigName);
    snapnow;
    % 加速度计误差
    accBiasErr = repmat(par_trajGen.IMU.acc.B', m, 1); % 加速度计的3个零偏误差
    accSFErr = repmat(par_trajGen.IMU.acc.dSF', m, 1); % 加速度计的3个标度因数误差
    accMAErr = repmat(offdiag2vec(par_trajGen.IMU.acc.MA)', m, 1); % 加速度计的6个失准角误差
    accSOErr = repmat(par_trajGen.IMU.acc.SO', m, 1); % 加速度计的3个二次项系数误差
    structAccPar(1) = acctriadpararr2struct([accBiasErr accSFErr accMAErr accSOErr], [NaN(1, 3) 1:15], false);
    structAccPar(2) = acctriadpararr2struct(out_intNavError.kalmFilt_x(:, 11:25), [NaN(1, 3) 1:15], false);
    plotacctriadpar(out_intNavError.kalmFilt_tExtrap, structAccPar, {'误差真值', '误差估计值'}, [], [], accParErrFigName);
    disp(accParErrFigName);
    snapnow;
    % 陀螺误差
    gyroBiasErr = repmat(par_trajGen.IMU.gyro.B', m, 1); % 陀螺的3个零偏误差
    gyroSFErr = repmat(par_trajGen.IMU.gyro.dSFp', m, 1); % 陀螺的3个标度因数误差
    gyroMAErr = repmat(offdiag2vec(par_trajGen.IMU.gyro.MA)', m, 1); % 陀螺的6个失准角误差
    structGyroPar(1) = gyrotriadpararr2struct([gyroBiasErr gyroSFErr gyroMAErr], [NaN(1, 3) 1:12 NaN(1, 9)], false);
    structGyroPar(2) = gyrotriadpararr2struct(out_intNavError.kalmFilt_x(:, 26:37), [NaN(1, 3) 1:12 NaN(1, 9)], false);
    plotgyrotriadpar(out_intNavError.kalmFilt_tExtrap, structGyroPar, {'误差真值', '误差估计值'}, [], [], gyroParErrFigName);
    disp(gyroParErrFigName);
    snapnow;
end